
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_matrix3.c
  * @author     baiyang
  * @date       2021-11-24
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gp_matrix3.h"
/*-----------------------------------macro------------------------------------*/
#ifndef min
#define min(a, b) ((a) > (b) ? (b) : (a))
#endif

#ifndef equal
#define equal(a, b) (((a) - (b)) < (1e-15) && ((a) - (b)) > -(1e-15))
#endif
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// create a rotation matrix given some euler angles
// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
void matrix3f_from_euler(matrix3f_t* mat, const float roll, const float pitch, const float yaw)
{
    const float cp = cosf(pitch);
    const float sp = sinf(pitch);
    const float sr = sinf(roll);
    const float cr = cosf(roll);
    const float sy = sinf(yaw);
    const float cy = cosf(yaw);

    mat->a.x = cp * cy;
    mat->a.y = (sr * sp * cy) - (cr * sy);
    mat->a.z = (cr * sp * cy) + (sr * sy);
    mat->b.x = cp * sy;
    mat->b.y = (sr * sp * sy) + (cr * cy);
    mat->b.z = (cr * sp * sy) - (sr * cy);
    mat->c.x = -sp;
    mat->c.y = sr * cp;
    mat->c.z = cr * cp;
}

/*
  fill the matrix from Euler angles in radians in 312 convention
*/
void matrix3f_from_euler312(matrix3f_t* mat, const float roll, const float pitch, const float yaw)
{
    const float c3 = cosf(pitch);
    const float s3 = sinf(pitch);
    const float s2 = sinf(roll);
    const float c2 = cosf(roll);
    const float s1 = sinf(yaw);
    const float c1 = cosf(yaw);

    mat->a.x = c1 * c3 - s1 * s2 * s3;
    mat->b.y = c1 * c2;
    mat->c.z = c3 * c2;
    mat->a.y = -c2*s1;
    mat->a.z = s3*c1 + c3*s2*s1;
    mat->b.x = c3*s1 + s3*s2*c1;
    mat->b.z = s1*s3 - s2*c1*c3;
    mat->c.x = -s3*c2;
    mat->c.y = s2;
}

// calculate euler angles from a rotation matrix
// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
void matrix3f_to_euler(const matrix3f_t* mat, float *roll, float *pitch, float *yaw)
{
    if (pitch != NULL) {
        *pitch = -math_asinf(mat->c.x);
    }
    if (roll != NULL) {
        *roll = atan2f(mat->c.y, mat->c.z);
    }
    if (yaw != NULL) {
        *yaw = atan2f(mat->b.x, mat->a.x);
    }
}

void matrix3f_from_rotation(matrix3f_t* mat, enum RotationEnum rotation)
{
    mat->a.x = 1.0f;
    mat->a.y = 0.0f;
    mat->a.z = 0.0f;

    mat->b.x = 0.0f;
    mat->b.y = 1.0f;
    mat->b.z = 0.0f;

    mat->c.x = 0.0f;
    mat->c.y = 0.0f;
    mat->c.z = 1.0f;

    vec3_rotate(&mat->a, rotation);
    vec3_rotate(&mat->b, rotation);
    vec3_rotate(&mat->c, rotation);

    matrix3f_transpose(mat);
}

void matrix3f_rotate(matrix3f_t* mat, const Vector3f_t* g)
{
    matrix3f_t delta = {
        {mat->a.y * g->z - mat->a.z * g->y, mat->a.z * g->x - mat->a.x * g->z, mat->a.x * g->y - mat->a.y * g->x},
        {mat->b.y * g->z - mat->b.z * g->y, mat->b.z * g->x - mat->b.x * g->z, mat->b.x * g->y - mat->b.y * g->x},
        {mat->c.y * g->z - mat->c.z * g->y, mat->c.z * g->x - mat->c.x * g->z, mat->c.x * g->y - mat->c.y * g->x}
    };

    matrix3f_add(mat, mat, &delta);
}

void matrix3f_zeros(matrix3f_t* mat)
{
    vec3_zero(&mat->a);
    vec3_zero(&mat->b);
    vec3_zero(&mat->c);
}

void matrix3f_eye(matrix3f_t* mat)
{
    matrix3f_zeros(mat);

    mat->a.x = 1.0f;
    mat->b.y = 1.0f;
    mat->c.z = 1.0f;
}

/* dst = src1 + src2 */
void matrix3f_add(matrix3f_t* dst, const matrix3f_t* src1, const matrix3f_t* src2)
{
    vec3_add(&dst->a, &src1->a, &src2->a);
    vec3_add(&dst->b, &src1->b, &src2->b);
    vec3_add(&dst->c, &src1->c, &src2->c);
}

/* dst = src1 - src2 */
void matrix3f_sub(matrix3f_t* dst, const matrix3f_t* src1, const matrix3f_t* src2)
{
    vec3_sub(&dst->a, &src1->a, &src2->a);
    vec3_sub(&dst->b, &src1->b, &src2->b);
    vec3_sub(&dst->c, &src1->c, &src2->c);
}

/* dst = src1 * src2 */
void matrix3f_mul(matrix3f_t* dst, const matrix3f_t* src1, const matrix3f_t* src2)
{
    matrix3f_t mat;

    mat.a.x = src1->a.x * src2->a.x + src1->a.y * src2->b.x + src1->a.z * src2->c.x;
    mat.a.y = src1->a.x * src2->a.y + src1->a.y * src2->b.y + src1->a.z * src2->c.y;
    mat.a.z = src1->a.x * src2->a.z + src1->a.y * src2->b.z + src1->a.z * src2->c.z;

    mat.b.x = src1->b.x * src2->a.x + src1->b.y * src2->b.x + src1->b.z * src2->c.x;
    mat.b.y = src1->b.x * src2->a.y + src1->b.y * src2->b.y + src1->b.z * src2->c.y;
    mat.b.z = src1->b.x * src2->a.z + src1->b.y * src2->b.z + src1->b.z * src2->c.z;

    mat.c.x = src1->c.x * src2->a.x + src1->c.y * src2->b.x + src1->c.z * src2->c.x;
    mat.c.y = src1->c.x * src2->a.y + src1->c.y * src2->b.y + src1->c.z * src2->c.y;
    mat.c.z = src1->c.x * src2->a.z + src1->c.y * src2->b.z + src1->c.z * src2->c.z;

    *dst = mat;
}

/* dst = m * v */
void matrix3f_mul_vec(Vector3f_t* dst, const matrix3f_t* m, const Vector3f_t* v)
{
    Vector3f_t vec = *v;

    dst->x = m->a.x * vec.x + m->a.y * vec.y + m->a.z * vec.z;
    dst->y = m->b.x * vec.x + m->b.y * vec.y + m->b.z * vec.z;
    dst->z = m->c.x * vec.x + m->c.y * vec.y + m->c.z * vec.z;
}

/* dst = src1 * src2 */
void matrix3f_mul_transpose_vec(Vector3f_t* dst, const matrix3f_t* m, const Vector3f_t* v)
{
    Vector3f_t vec = *v;

    dst->x = m->a.x * vec.x + m->b.x * vec.y + m->c.x * vec.z;
    dst->y = m->a.y * vec.x + m->b.y * vec.y + m->c.y * vec.z;
    dst->z = m->a.z * vec.x + m->b.z * vec.y + m->c.z * vec.z;
}

/* dst = src' */
void matrix3f_trans(matrix3f_t* dst, const matrix3f_t* src)
{
    matrix3f_t mat;

    mat.a.x = src->a.x;
    mat.a.y = src->b.x;
    mat.a.z = src->c.x;

    mat.b.x = src->a.y;
    mat.b.y = src->b.y;
    mat.b.z = src->c.y;

    mat.c.x = src->a.z;
    mat.c.y = src->b.z;
    mat.c.z = src->c.z;

    *dst = mat;
}

matrix3f_t matrix3f_transposed(const matrix3f_t* mat)
{
    matrix3f_t mat_3f;

    mat_3f.a.x = mat->a.x;
    mat_3f.a.y = mat->b.x;
    mat_3f.a.z = mat->c.x;

    mat_3f.b.x = mat->a.y;
    mat_3f.b.y = mat->b.y;
    mat_3f.b.z = mat->c.y;

    mat_3f.c.x = mat->a.z;
    mat_3f.c.y = mat->b.z;
    mat_3f.c.z = mat->c.z;

    return mat_3f;
}

void matrix3f_normalize(matrix3f_t* mat)
{
    const float error = vec3_dot(&mat->a, &mat->b);
    const Vector3f_t t0 = {mat->a.x - (mat->b.x * (0.5f * error)),
                           mat->a.y - (mat->b.y * (0.5f * error)),
                           mat->a.z - (mat->b.z * (0.5f * error))};

    const Vector3f_t t1 = {mat->b.x - (mat->a.x * (0.5f * error)),
                           mat->b.y - (mat->a.y * (0.5f * error)),
                           mat->b.z - (mat->a.z * (0.5f * error))};

    Vector3f_t t2;
    vec3_cross(&t2, &t0, &t1);

    vec3_mult(&mat->a, &t0, (1.0f / vec3_length(&t0)));
    vec3_mult(&mat->b, &t1, (1.0f / vec3_length(&t1)));
    vec3_mult(&mat->c, &t2, (1.0f / vec3_length(&t2)));
}
/*------------------------------------test------------------------------------*/


